package it.unibo.refolding.pico;

import it.unibo.refolding.common.CommonThings;
import it.unibo.refolding.common.MyRuntimeException;
import it.unibo.refolding.common.Sleeper;
import it.unibo.refolding.pico.FileMovingWatcher.FileContentAndTargetFile;
import it.unibo.refolding.pico.stripchart.JpHelper;
import it.unimi.dsi.fastutil.doubles.DoubleList;
import it.unimi.dsi.fastutil.ints.IntList;

import java.io.IOException;
import java.nio.file.Files;
import java.nio.file.Path;
import java.util.Calendar;
import java.util.logging.Level;
import java.util.logging.Logger;

import com.google.common.base.Preconditions;

public class StripChartRefoldingNanoAdapter extends AbstractRefoldingNanoAdapter {

//  private static class RepeatFunctionInput {
//    public RepeatFunctionInput(String prefix, String suffix, double timeS) {
//      this.prefix = prefix;
//      this.suffix = suffix;
//      this.timeS = timeS;
//    }
//    
//    private final String prefix, suffix;
//    private final double timeS;
//  }
//  
//	private class RepeatFunction implements Function<RepeatFunctionInput, TimedFileContentAndTargetFile> {
//    @Override
//    public TimedFileContentAndTargetFile apply(RepeatFunctionInput input) {
//      long startNanoTime = System.nanoTime();
//      nanoDriver.runScript();
//      TimedFileContentAndTargetFile fileContentAndTargetFile = RefoldingUtil.afterScriptRun(
//        startNanoTime, input.prefix, input.suffix, input.timeS, true, nanoDriver, fileMovingWatcher);
//      nanoDriver.resetStripChart();
//      return fileContentAndTargetFile;
//    }
//  }

  private static final Logger logger = Logger.getLogger(StripChartRefoldingNanoAdapter.class
		.getSimpleName());

  public static final int FETCH_MOLECULE_JUNCTIONS = 6;
  private final double STARTING_TIME_S = 0.35;
  private final SingleScriptSetup latestSingleScriptSetup;

  private final double pushM, hoverM;
  private final double parkingHoverM;
  private final double parkingVelocityMPerS;
  private final double refoldApproachVelMPerS;

  private double initialParkingPositionM = Double.NaN;
  private double zScanStartM = Double.NaN;

  private final int refoldCount;

  private final DoubleList refoldAllowedTimeRelativeError;
//  private final Repeater<RepeatFunctionInput, TimedFileContentAndTargetFile> repeater;

	protected StripChartRefoldingNanoAdapter(double[] revVels, double breakAwayRampSizeM,
		double fetchMoleculeRampSizeM, double fetchMoleculeSurfaceDelayS,
		double pushM,
		double hoverM, double parkingHoverM, double refoldApproachVelMPerS, int refoldLoopCount,
		double loseRampSizeM, double loseVelocityMPerS, double parkingVelocityMPerS,
		DoubleList allowedTimeRelativeError, DoubleList refoldAllowedTimeRelativeError, Path junctionPointsDir,
		Sleeper sleeper, Sleeper nanoDriverSleeper, FileMovingWatcher fileMovingWatcher) {
		super(
			revVels, breakAwayRampSizeM, fetchMoleculeRampSizeM, fetchMoleculeSurfaceDelayS,
			loseRampSizeM, loseVelocityMPerS, allowedTimeRelativeError, junctionPointsDir, sleeper, nanoDriverSleeper,
			fileMovingWatcher);
		Preconditions.checkArgument(refoldLoopCount >= 1);
		this.refoldAllowedTimeRelativeError = refoldAllowedTimeRelativeError;
		this.pushM = pushM;
		this.hoverM = hoverM;
		this.parkingHoverM = parkingHoverM;
		this.parkingVelocityMPerS = parkingVelocityMPerS;
		this.refoldApproachVelMPerS = refoldApproachVelMPerS;
		this.refoldCount = refoldLoopCount;
		this.latestSingleScriptSetup = new SingleScriptSetup();
//		this.repeater = new Repeater<RepeatFunctionInput, TimedFileContentAndTargetFile>(new RepeatFunction());
//		Executors.newSingleThreadExecutor().execute(repeater);
	}

  @Override
	public PreviousAndCurrentLists breakAway(String prefix, String suffix) {
		try {
			return PreviousAndCurrentLists.of(performBreakAway(prefix, suffix));
		} catch (IOException e) {
			throw new MyRuntimeException(e.getMessage(), e);
		}
	}

	public static ApproachRetractionLoop upDownLoop(
		double approachRampSizeM, double approachVelocityMPerS,
		double retractionRampSizeM, double retractionVelocityMPerS,
		StripChartPicoData fetchStripChart, IntList jp) {
		StripChartPicoData approachRetractionStripChart = fetchStripChart.subRange(0, jp.getInt(4));
		ApproachRetractionLoop approachRetractionLoop = PicoDataExtractor
			.createLoopGivenJunctionPoints(
				approachRetractionStripChart, approachRampSizeM, approachVelocityMPerS,
				retractionRampSizeM, retractionVelocityMPerS, 1, jp.subList(0, 4));
		return approachRetractionLoop;
	}

	private ParsedAndTargetFile performBreakAway(String prefix, String suffix) throws IOException {
	  if (Double.isNaN(initialParkingPositionM)) {
	    /*
	     * Start/Pause is only needed to make Quit work properly, if there is actually no stripchart to quit from.
	     */
	    // These two actions help restart cleanly, but also after failures.
	    nanoDriver.scriptMode();
	    nanoDriver.startStripChart();  // Start/Pause
	    nanoDriver.resetStripChart();
	    sleepAfterStartingStripChart();
	  }
	  return performLoop(
			0, pushM, breakAwayRampSizeM, defaultRevVelMPerS,
			PicoConstants.DEAD_TIME_S, breakAwayRampSizeM, defaultRevVelMPerS, 1, allowedTimeRelativeError, prefix, suffix);
	}

  public void sleepAfterStartingStripChart() {
    CommonThings.sleep((long)(STARTING_TIME_S * 1000), sleeper);
  }

	private ParsedAndTargetFile performLoop(
		double startingTimeEstimateS, double pushM, double approachRampSizeM, double approachVelocityMPerS,
		double surfaceDelayS, double retractionRampSizeM, double retractionVelocityMPerS,
		int iterations,
		DoubleList allowedTimeRelativeError, String prefix, String suffix) throws IOException {
	  // Optional.
//	  nanoDriver.quitStripChart();
	  double initialMovementM = -(approachRampSizeM - pushM - parkingHoverM);
	  double initialMovementS = Math.abs(initialMovementM) / PicoConstants.MAX_VELOCITY_M_PER_S;
	  // initialMovement is normally negative, but if parkingNm > hoverNm +
	  // approachRampSizeM (or retractionRampSizeM), then it could be the opposite.
	  double parkingMovementM = (retractionRampSizeM - pushM - parkingHoverM);
	  double rectificationTimeS = PicoConstants.DEAD_TIME_S
      + (parkingMovementM == 0 ? 0 : Math.abs(parkingMovementM) / parkingVelocityMPerS
        + PicoConstants.DEAD_TIME_S) + initialMovementS;
    double expectedTimeS = approachRampSizeM
      / approachVelocityMPerS
      + surfaceDelayS
      + retractionRampSizeM
      / retractionVelocityMPerS
      + rectificationTimeS;
//    double finalMovementS = Math.abs(parkingMovementNm) / parkingVelocityMPerS;
    // I'm not sure this is very accurate, but I don't care in fact that much.
    double accurateTimeS = initialMovementS + PicoConstants.DEAD_TIME_S
      + expectedTimeS * iterations
      + initialMovementS * (iterations - 1);
    SingleScriptSetup setup = new SingleScriptSetup(
      initialMovementM, approachRampSizeM, approachVelocityMPerS, surfaceDelayS,
      retractionVelocityMPerS, retractionRampSizeM, parkingVelocityMPerS, parkingMovementM, iterations);
    long startNanoTime = setupSingleScriptAndRun(
      latestSingleScriptSetup.indicateWhenUnchanged(setup), prefix, nanoDriver);
    latestSingleScriptSetup.copyFrom(setup);
    double timeS = Math.max(startingTimeEstimateS, accurateTimeS);
    CommonThings.sleep((long) Math.round(timeS * 1000), nanoDriverSleeper);
    TimedFileContentAndTargetFile fileContentAndTargetFile = RefoldingUtil.afterScriptRun(
      startNanoTime, prefix, suffix, true, nanoDriver, fileMovingWatcher);
		Parsed parsed = parseLoop(
			fileContentAndTargetFile, approachRampSizeM, approachVelocityMPerS, surfaceDelayS,
			retractionRampSizeM, retractionVelocityMPerS, rectificationTimeS, iterations, allowedTimeRelativeError, prefix);
		return new ParsedAndTargetFile(parsed, fileContentAndTargetFile.targetFile());
	}

	/**
	 * @param parkingDeltaM
	 *            From the outside, it is considered that the first curve has
	 *            parkingPositionM = 0.
	 * @param prefix
	 * @return
	 * @throws IOException
	 */
	private ParsedAndTargetFile performFetchMolecule(
		double parkingDeltaM, String prefix, String suffix) throws IOException {
		updateZScanStartM(parkingDeltaM, prefix + "_ZScanStart_", suffix);
		return performLoop(
		  0, pushM, fetchMoleculeRampSizeM, defaultRevVelMPerS,
			fetchMoleculeSurfaceDelayS, fetchMoleculeRampSizeM, defaultRevVelMPerS,
			1, allowedTimeRelativeError, prefix, suffix);
	}

	private ParsedAndTargetFile performRefold(
		double approachVel, double revVel,
		double parkingDeltaM, double refoldApproachRampSizeM, double refoldRetractionRampSizeM, double surfaceDelayS,
		String prefix, String suffix) throws IOException {
		updateZScanStartM(parkingDeltaM, "", "");
    return performLoop(
      0, -hoverM, refoldApproachRampSizeM, approachVel, surfaceDelayS, refoldRetractionRampSizeM,
      revVel, refoldCount, refoldAllowedTimeRelativeError, prefix, suffix);
	}

	public Parsed parseLoop(
	  TimedFileContentAndTargetFile fileContentAndTargetPath, double approachRampSizeM,
		double approachVelocityMPerS, double surfaceDelayS,
		double retractionRampSizeM, double retractionVelocityMPerS,
		double rectificationTimeS,
		int iterations, DoubleList allowedTimeRelativeError,
		String prefix) throws IOException {
    StripChartPicoData stripChart = (StripChartPicoData) PicoDataExtractor.parse(fileContentAndTargetPath.fileContent());
    if (stripChart == null) {
      return Parsed.NOT_PARSED;
    }
    logger.log(
      Level.INFO, "totalTime(s)={0} probable(s)={1} upperBound(s)={2}",
      new Object[] {
        stripChart.timeInstantCount() / (double) stripChart.stripChartRateHz, fileContentAndTargetPath.probableTimeS,
        fileContentAndTargetPath.upperBoundTimeS});
		int maxPoints = (int) Math.floor(fileContentAndTargetPath.upperBoundTimeS * stripChart.stripChartRateHz);
    LoopParams params = new LoopParams(
      approachRampSizeM, approachVelocityMPerS, surfaceDelayS, retractionRampSizeM,
      retractionVelocityMPerS, rectificationTimeS, iterations, true);
		// Optimisation: only look at the last maxPoints data points.
		IntList jp = readOrWriteApproachRetractionLoopJunctionPoints(
	    stripChart, Math.max(stripChart.timeInstantCount() - maxPoints, 0), stripChart.timeInstantCount(),
	    fileContentAndTargetPath.targetFile(), params, allowedTimeRelativeError, junctionPointsDir);
    return parseApproachRetraction(stripChart, jp, prefix, "");
	}
	
  static Parsed parseApproachRetraction(
		StripChartPicoData stripChart, IntList jp, String prefix, String suffix) {
		if (stripChart == null) {
			return Parsed.NOT_PARSED;
		}
		final Parsed parsed;
		if (jp == null) {
			parsed = Parsed.NOT_PARSED;
		} else {
		  parsed = new Parsed(stripChart.sharedApproach().sharedZList(), stripChart.sharedApproach().sharedDeflectionList(),
	      jp,
	      PicoUtil.stripChartToPlainPicoDataList(stripChart, jp));
		}
		return parsed;
	}

  public static IntList readOrWriteApproachRetractionLoopJunctionPoints(
    StripChartPicoData stripChart, int from, int to, Path targetFile, LoopParams params,
    DoubleList allowedTimeRelativeError,
    Path junctionPointsDir) throws IOException {
    IntList jp;
		// Works even when junctionPointsDir is null.
		Path junctionPointsFile = JpHelper.junctionPointsFile(junctionPointsDir, targetFile);
		if (junctionPointsFile == null || !Files.isRegularFile(junctionPointsFile)) {
			jp = PicoDataExtractor.computeLoopJunctionPoints(
				stripChart, from, to, params, allowedTimeRelativeError);
			if (junctionPointsDir != null & jp != null) {
				JpHelper.writeToFile(jp, junctionPointsFile);
			}
		} else {
			jp = JpHelper.extractJunctionPoints(junctionPointsFile);
			Preconditions.checkState(jp.size() == 4 * params.iterations);
		}
    if (jp == null) {
      logger.log(
        Level.WARNING,
        "Could not fit junction points of {0} with params={1}, allowedTimeRelativeError={7}.",
        new Object[] {targetFile, params, allowedTimeRelativeError});
    }
    return jp;
  }

	private void updateZScanStartM(double parkingM, String prefix, String suffix) {
		logger.log(Level.FINER, "parkingNm={0}", parkingM * 1e9);
		boolean forceMode = !(zScanStartM == initialParkingPositionM + parkingM);
		if (forceMode) {
		  nanoDriver.startStripChartQuitStripChartForceMode();
		} else {
      logger.log(
        Level.FINE, "Not switching to Force Mode because zScanStartM(nm)={0}.", zScanStartM * 1e9);
		}
		boolean wasNaN = Double.isNaN(initialParkingPositionM);
    if (wasNaN) {
      // to restart more easily
      nanoDriver.startStripChart(); // Start/Pause
      nanoDriver.quitStripChart();
			nanoDriver.single();
			nanoDriver.captureButton();
			FileContentAndTargetFile fct = fileMovingWatcher.latestFileContentAndTargetFile(
				prefix, suffix, true);
			PlainPicoData picoData = (PlainPicoData) PicoDataExtractor.parse(fct.fileContent);
			initialParkingPositionM = zScanStartM = picoData.zScanStartMetres();
		}
		if (forceMode) {
  		nanoDriver.zScanStartMScriptModeStartStripChart(zScanStartM = initialParkingPositionM + parkingM);
		} else if (wasNaN) {
      nanoDriver.scriptMode();
      nanoDriver.startStripChart();
		}
		if (forceMode | wasNaN) {
		  sleepAfterStartingStripChart();
		}
	}

  static long setupSingleScriptAndRun(
    SingleScriptSetup setup, String prefix, NanoDriver nanoDriver) {
    setupSingleScript(setup, nanoDriver);
    long startNanos = System.nanoTime();
    nanoDriver.runScript();
    return startNanos;
  }
  
  static void setupSingleScript(SingleScriptSetup setup, NanoDriver nanoDriver) {
    // Column 1 must be Absolute.
    nanoDriver.setupSingleScript(
      setup.initialMovementM, setup.initialMovementM + setup.approachRampSizeM, setup.approachVelocityMPerS, setup.surfaceDelayS
        - PicoConstants.DEAD_TIME_S, setup.retractionVelocityMPerS, -setup.retractionRampSizeM,
        setup.parkingVelocityMPerS, setup.parkingMovementM,
        setup.count - 1);
  }
	 
	@Override
	public NanoDriverConfig createRobotConfig(boolean forceRecord) {
		return NanoDriverUtil.createStripChartRefoldingRobotConfig(forceRecord);
	}

	@Override
	public ParsedAndTargetFile fetchMolecule(double parkingDeltaM, String prefix, String suffix) {
		try {
			return performFetchMolecule(parkingDeltaM, prefix, suffix);
		} catch (IOException e) {
			throw new MyRuntimeException(e.getMessage(), e);
		}
	}

	@Override
	public PreviousAndCurrentLists refold(
		int velIndex,
		double parkingDeltaM, double refoldApproachRampSizeM, double refoldRetractionRampSizeM, double surfaceDelayS,
		String prefix, String suffix) {
	  ParsedAndTargetFile parsedAndTargetFile;
		try {
      parsedAndTargetFile = performRefold(
        refoldApproachVelMPerS, velIndex == -1 ? defaultRevVelMPerS : reverseVelocity(velIndex), parkingDeltaM,
        refoldApproachRampSizeM, refoldRetractionRampSizeM, surfaceDelayS, prefix, suffix);
		} catch (IOException e) {
			throw new MyRuntimeException(e.getMessage(), e);
		}
		return PreviousAndCurrentLists.of(parsedAndTargetFile);
	}

	static String filenamePrefix(String prefix) {
		Calendar now = Calendar.getInstance();
		// %F and %T do not work
		return String.format("%1$s%2$tY%2$tm%2$td_%2$tH%2$tM%2$tS", prefix, now);
	}

  @Override
  public void xOffsetM(double xOffsetM) {
    /*
     * If you want to do quitStripChart, then make sure to press first
     * Start/Pause.
     */
    nanoDriver.forceModeXOffsetScriptMode(xOffsetM);
//    CommonThings.sleep(500, sleeper);
  }

	@Override
	public void ping() {
		nanoDriver.ping();
	}

	@Override
	public PreviousAndCurrentLists lose(
		double approachRampSizeM, double retractionRampSizeM, double parkingDeltaM, String prefix, String suffix) {
		try {
			return PreviousAndCurrentLists.of(performLose(
			  approachRampSizeM, parkingDeltaM, prefix, suffix));
		} catch (IOException e) {
			throw new MyRuntimeException(e.getMessage(), e);
		}
	}

	private PlainPicoDataAndTargetFile performLose(
		double approachRampSizeM, double parkingDeltaM, String prefix, String suffix)
		throws IOException {
		updateZScanStartM(parkingDeltaM, "", "");
		ParsedAndTargetFile parsedAndTargetFile = performLoop(
			0, -hoverM, approachRampSizeM, loseVelocityMPerS,
			PicoConstants.DEAD_TIME_S, loseRampSizeM, loseVelocityMPerS,
			1, refoldAllowedTimeRelativeError, prefix, suffix);
		return parsedAndTargetFile.emptyOrOneToParsedAndTargetFile();
	}
}
